#define _CRT_SECURE_NO_WARNINGS 1
#include<stdio.h>;

typedef struct{
    int current_pianyi;
    int last_pianyi;
    float kp;
    float ki;
    float kd;
    float integral;
    float max_integral;
    float moto;
    float max_moto;
}PID;



void pid_Init(PID* pid, float p, float i, float d,float maxi, float maxmoto)
{
    pid->current_pianyi = 0;
    pid->last_pianyi;
    pid->kp = p;
    pid->ki = i;
    pid->kd = d;
    pid->integral = 0;
    pid->max_integral = maxi;
    pid->moto = 0;
    pid->max_moto = maxmoto;
}

void calpid(PID* pid, int pyi)
{
    pid->last_pianyi = pid->current_pianyi;
    pid->current_pianyi = pyi;
    pid->integral += pid->current_pianyi * pid->ki;
    if (pid->integral > pid->max_integral) pid->integral = pid->max_integral;
    else if (pid->integral < -pid->max_integral) pid->integral = -pid->max_integral;
    pid->moto = pid->kp * pid->current_pianyi + pid->integral + (pid->current_pianyi - pid->last_pianyi) * pid->kd;
    if (pid->moto > pid->max_moto) pid->moto = pid->max_moto;
    if (pid->moto <-pid->max_moto) pid->moto = -pid->max_moto;
}

PID left;
PID right;

int main()
{
    pid_Init(&left, 0.1, 0, 0.05, 100, 300);
    pid_Init(&right, 0.1, 0, 0.05, 100, 300);
    float pyi = -10;
    //scanf("%.2f", pyi);
    do {
        if (pyi < 0)
        {
            calpid(&left, pyi);
            pyi -= left.moto;
            printf("moto:%.2f\n", left.moto);
            printf("pyi:%.2f\n", pyi);
            printf("\n");
        }
        else if (pyi > 0)
        {
            calpid(&right, -pyi);
            pyi += right.moto;
            printf("moto:%.2f\n", right.moto);
            printf("pyi:%.2f\n", pyi);
            printf("\n");
        }
        
    } while (pyi < -1 || pyi > 1);

}

